=======================
Artificial Intelligence
=======================
This section is allocated to the AI and robot modeling algorithms that will be discussed further later!

Robots Kinematic models
-----------------------
In this section I will describe the kinematic models of the robots and provide a code for estimating the end-effector position and etc.

UR5 Robot
^^^^^^^^^
.. code-block:: python

	#Math and numpy library import
	from math import *
	import numpy as np

	#Translation matrixes for x and z
	def TXZ(a,b):
		if b == 'x':
			return [[1, 0, 0, a], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]
		elif b == 'z':
			return [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, a], [0, 0, 0, 1]]

	#Rotation matrix for x and z
	def RXZ(a,b):
		c, s = cos(a), sin(a)
		if b == 'x':
			return [[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0], [0, 0, 0, 1]]
		if b== 'z':
			return [[c, -s, 0, 0], [s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]

	#Euler angles calcluations to obtain Roll Pitch Yaw in degree
	def HM2RPY(HM):
		RX = atan2(HM[2][1], HM[2][2])
		RZ = atan2(HM[1][0], HM[0][0])
		RY = atan2(-HM[2][0], HM[0][0] * cos(RZ) + HM[1][0] * sin(RZ))
		return [RX, RY, RZ]

	#DH table definition
	alpha1,alpha2,alpha3,alpha4,alpha5,alpha6=radians(90),radians(0),radians(0),radians(90),radians(-90),radians(0)
	a1,a2,a3,a4,a5,a6=0,-425,-392.25,0,0,0
	d1,d2,d3,d4,d5,d6=0,0,0,109.15,94.65,82.3

	#Joints angle definition
	j1,j2,j3,j4,j5,j6 = radians(23),radians(63),radians(33),radians(19),radians(10),radians(36.55)

	#Homogenous Matrix defnition
	tbase =TXZ(89.159,'z')
	t1 = np.matmul(np.matmul(np.matmul(TXZ(d1,'z'),RXZ(j1,'z')),TXZ(a1,'x')),RXZ(alpha1,'x'))
	t2 = np.matmul(np.matmul(np.matmul(TXZ(d2,'z'),RXZ(j2,'z')),TXZ(a2,'x')),RXZ(alpha2,'x'))
	t3 = np.matmul(np.matmul(np.matmul(TXZ(d3,'z'),RXZ(j3,'z')),TXZ(a3,'x')),RXZ(alpha3,'x'))
	t4 = np.matmul(np.matmul(np.matmul(TXZ(d4,'z'),RXZ(j4,'z')),TXZ(a4,'x')),RXZ(alpha4,'x'))
	t5 = np.matmul(np.matmul(np.matmul(TXZ(d5,'z'),RXZ(j5,'z')),TXZ(a5,'x')),RXZ(alpha5,'x'))
	t6 = np.matmul(np.matmul(np.matmul(TXZ(d6,'z'),RXZ(j6,'z')),TXZ(a6,'x')),RXZ(alpha6,'x'))

	#Homogenous transfer matrix
	ht=np.matmul(np.matmul(np.matmul(np.matmul(np.matmul(np.matmul(tbase,t1),t2),t3),t4),t5),t6)

	#Print the Homogenous Matrix
	print(ht)

	#Print the location 
	Orientation = HM2RPY(ht)
	print("X:", ht[0][3])
	print("Y:", ht[1][3])
	print("Z:", ht[2][3])

	print("--------------------------")
	#Print the orientation
	print("RX:", np.rad2deg(Orientation[0]))
	print("RY:", np.rad2deg(Orientation[1]))
	print("RZ:", np.rad2deg(Orientation[2]))

The descriptive model above estimates the orientation and transformation of a UR5 colaborative robot.
Meaning that for each joint space configuration, there is a corresponding cartesian orientation and location. 

Random Robot Joints configuration and generation
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
.. code-block:: cpp
	
	// Example C++ program
	#include <iostream>
	#include <string>
	#include <random>
	float randFloat(float a, float b)
	{
		//std::cout << (float)rand() << "\n";
		
		return ((b - a) * ((float)rand() / RAND_MAX)) + a;
	}
	int main()
	{
		float j1, j2, j3, j4, j5, j6;
		int nbExec = 10;
		
		for (int i=0; i<nbExec; i++)
		{
			j1 = round(10 * randFloat(-180, 40)) / 10;
			j2 = round(10 * randFloat(-150,  0)) / 10;
			j3 = round(10 * randFloat(-150,  0)) / 10;
			j4 = round(10 * randFloat(-180,  0)) / 10;
			j5 = round(10 * randFloat(-180, 90)) / 10;
			j6 = round(10 * randFloat( -90, 90)) / 10;
			std::cout << "J1=" <<  j1 << " " << "J2=" <<  j2 << " "  <<"J3=" <<  j3 << " "  <<"J4=" <<  j4 << " "  <<"J5=" <<  j5 << " "  <<"J6=" <<  j6 << " "  <<"\n";
		}
			
	}

The above code will generate uniformly random joints between a specific range.